import os
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch.launch_description_sources import PythonLaunchDescriptionSource
#该程序用于将激光雷达/里程计数据与地图匹配，通过nav2利用AMCL进行导航

def generate_launch_description():
    # 获取与拼接默认路径
    fishbot_navigation2_dir = get_package_share_directory(
        'fishbot_navigation2')
    nav2_bringup_dir = get_package_share_directory('nav2_bringup')
    rviz_config_dir = os.path.join(
        nav2_bringup_dir, 'rviz', 'nav2_default_view.rviz')
    
    # 创建 Launch 配置
    use_sim_time = launch.substitutions.LaunchConfiguration(
        'use_sim_time', default='true')
    map_yaml_path = launch.substitutions.LaunchConfiguration(
        'map', default=os.path.join(fishbot_navigation2_dir, 'maps', 'room.yaml'))
    nav2_param_path = launch.substitutions.LaunchConfiguration(
        'params_file', default=os.path.join(fishbot_navigation2_dir, 'config', 'nav2_params.yaml'))



    return launch.LaunchDescription([
        # 声明新的 Launch 参数
        launch.actions.DeclareLaunchArgument('use_sim_time', default_value=use_sim_time,
                                             description='Use simulation (Gazebo) clock if true'),
        launch.actions.DeclareLaunchArgument('map', default_value=map_yaml_path,
                                             description='Full path to map file to load'),
        launch.actions.DeclareLaunchArgument('params_file', default_value=nav2_param_path,
                                             description='Full path to param file to load'),

        launch.actions.IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                [nav2_bringup_dir, '/launch', '/bringup_launch.py']),
            # 使用 Launch 参数替换原有参数
            launch_arguments={
                'map': map_yaml_path,
                'use_sim_time': use_sim_time,
                'params_file': nav2_param_path}.items(),
        ),
        launch_ros.actions.Node(
            package='rviz2',
            executable='rviz2',
            name='rviz2',
            arguments=['-d', rviz_config_dir],
            parameters=[{'use_sim_time': use_sim_time}],
            output='screen'),
    ])


# #更改后也不能用
# import os
# import launch
# import launch_ros
# from ament_index_python.packages import get_package_share_directory
# from launch.launch_description_sources import PythonLaunchDescriptionSource

# def generate_launch_description():
#     # 获取功能包路径
#     fishbot_navigation2_dir = get_package_share_directory('fishbot_navigation2')
#     nav2_bringup_dir = get_package_share_directory('nav2_bringup')

#     # 使用 os.path.join 拼接路径
#     rviz_config_path = os.path.join(
#         nav2_bringup_dir,
#         'rviz',
#         'nav2_default_view.rviz'
#     )

#     default_map_path = os.path.join(
#         fishbot_navigation2_dir,
#         'maps',
#         'room.yaml'
#     )

#     default_nav2_params_path = os.path.join(
#         fishbot_navigation2_dir,
#         'config',
#         'nav2_params.yaml'
#     )

#     nav2_launch_file = os.path.join(
#         nav2_bringup_dir,
#         'launch',
#         'bringup_launch.py'
#     )

#     # 创建 Launch 配置
#     use_sim_time = launch.substitutions.LaunchConfiguration(
#         'use_sim_time',
#         default='true'
#     )
    
#     map_yaml_path = launch.substitutions.LaunchConfiguration(
#         'map',
#         default=default_map_path
#     )
    
#     nav2_param_path = launch.substitutions.LaunchConfiguration(
#         'params_file',
#         default=default_nav2_params_path
#     )

#     # 添加路径存在性检查
#     if not os.path.exists(default_map_path):
#         raise FileNotFoundError(f"Map file not found at: {default_map_path}")
#     if not os.path.exists(default_nav2_params_path):
#         raise FileNotFoundError(f"Nav2 params file not found at: {default_nav2_params_path}")
#     if not os.path.exists(rviz_config_path):
#         raise FileNotFoundError(f"RViz config file not found at: {rviz_config_path}")

#     return launch.LaunchDescription([
#         # 声明 Launch 参数
#         launch.actions.DeclareLaunchArgument(
#             'use_sim_time',
#             default_value=use_sim_time,
#             description='Use simulation (Gazebo) clock if true'
#         ),
#         launch.actions.DeclareLaunchArgument(
#             'map',
#             default_value=map_yaml_path,
#             description='Full path to map file to load'
#         ),
#         launch.actions.DeclareLaunchArgument(
#             'params_file',
#             default_value=nav2_param_path,
#             description='Full path to param file to load'
#         ),

#         # 包含 nav2_bringup 启动文件
#         launch.actions.IncludeLaunchDescription(
#             PythonLaunchDescriptionSource(nav2_launch_file),
#             launch_arguments={
#                 'map': map_yaml_path,
#                 'use_sim_time': use_sim_time,
#                 'params_file': nav2_param_path
#             }.items()
#         ),

#         # 启动 RViz2
#         launch_ros.actions.Node(
#             package='rviz2',
#             executable='rviz2',
#             name='rviz2',
#             arguments=['-d', rviz_config_path],
#             parameters=[{'use_sim_time': use_sim_time}],
#             output='screen'
#         ),
#     ])